// Copyright 2008 Isis Innovation Limited
#include "KeyFrame.h"
#include "ShiTomasi.h"
#include "Scarf.h"
#include "SmallBlurryImage.h"
#include "LevelHelpers.h"
#include <cvd/vision.h>
#include <cvd/fast_corner.h>
#include <cvd/colourspace_convert.h>
#include <cvd/colourspaces.h>

using namespace CVD;
using namespace std;
using namespace GVars3;

KeyFrame::~KeyFrame()
{
  if(pSBI != NULL)
    delete pSBI;

  //if(gluTextureId != 0)
    //glDeleteTextures(1,&gluTextureId); //TODO: why does this cause a segmentation fault!
}

void KeyFrame::MakeKeyFrame_Lite(CVD::Image<CVD::byte> &grey, int (&nLevelThreshFAST)[LEVELS], SE3GaussianPDF &se3PdfGfromC)
{
  static gvar3<double> gvnMinLevelCorners("KeyFrame.MinLevelCorners", 1000, SILENT);
  static gvar3<double> gvnMaxLevelCorners("KeyFrame.MaxLevelCorners", 5000, SILENT);

  mse3PdfGfromC = se3PdfGfromC;

  // Perpares a Keyframe from an image. Generates pyramid levels, does FAST detection, etc.
  // Does not fully populate the keyframe struct, but only does the bits needed for the tracker;
  // e.g. does not perform FAST nonmax suppression. Things like that which are needed by the 
  // mapmaker but not the tracker go in MakeKeyFrame_Rest();
  
  // First, copy out the image data to the pyramid's zero level.
  aLevels[0].im.resize(grey.size());
  copy(grey, aLevels[0].im);

  // Then, for each level...
  for(int i=0; i<LEVELS; i++)
    {
      Level &lev = aLevels[i];
      if(i!=0)
	    {  // .. make a half-size image from the previous level..
	      lev.im.resize(aLevels[i-1].im.size() / 2);
	      halfSample(aLevels[i-1].im, lev.im);
	    }
      
      // .. and detect and store FAST corner points.
      // I use a different threshold on each level; this is a bit of a hack
      // whose aim is to balance the different levels' relative feature densities.
      lev.vCorners.clear();
      lev.vCandidates.clear();
      lev.vMaxCorners.clear();
      if(i == 0)
	      fast_corner_detect_10(lev.im, lev.vCorners, nLevelThreshFAST[i]);//10
      if(i == 1)
	      fast_corner_detect_10(lev.im, lev.vCorners, nLevelThreshFAST[i]);//15
      if(i == 2)
	      fast_corner_detect_10(lev.im, lev.vCorners, nLevelThreshFAST[i]);//15
      if(i == 3)
	      fast_corner_detect_10(lev.im, lev.vCorners, nLevelThreshFAST[i]);//10
      
      if(lev.vCorners.size() > *gvnMaxLevelCorners)
        nLevelThreshFAST[i] += 1;
      else if(lev.vCorners.size() < *gvnMinLevelCorners)
        nLevelThreshFAST[i] = max(5, nLevelThreshFAST[i]-1);

      // Generate row look-up-table for the FAST corner points: this speeds up 
      // finding close-by corner points later on.
      unsigned int v=0;
      lev.vCornerRowLUT.clear();
      for(int y=0; y<lev.im.size().y; y++)
	    {
	      while(v < lev.vCorners.size() && y > lev.vCorners[v].y)
	        v++;
	      lev.vCornerRowLUT.push_back(v);
	    }
    };

    //std::cout << aLevels[0].vCorners.size() << " " << aLevels[1].vCorners.size() << " " << aLevels[2].vCorners.size() << " " << aLevels[3].vCorners.size() << " " << nLevelThreshFAST[0] << " " << nLevelThreshFAST[1] << " " << nLevelThreshFAST[2] << " " << nLevelThreshFAST[3] << std::endl;
}

void KeyFrame::MakeKeyFrame_Rest(Scarf &s)
{
  // Fills the rest of the keyframe structure needed by the mapmaker:
  // FAST nonmax suppression, generation of the list of candidates for further map points,
  // creation of the relocaliser's SmallBlurryImage.
  static gvar3<double> gvdCandidateMinSTScore("KeyFrame.CandidateMinShiTomasiScore", 70, SILENT);
  static gvar3<double> gvdCandidateMinScaleResponseScore("KeyFrame.CandidateMinScaleResponseScore", 10, SILENT);
  static gvar3<double> gvdCandidateNumScales("KeyFrame.CandidateNumScales", 8, SILENT);
  static gvar3<int> gvnMaxLevelCandidates("KeyFrame.MaxLevelCandidates", 500, SILENT);

  s.LoadImage(aLevels[0].im);

  // For each level...
  for(int l=0; l<LEVELS; l++)
  {
    Level &lev = aLevels[l];
    // .. find those FAST corners which are maximal..
    //    fast_nonmax(lev.im, lev.vCorners, 10, lev.vMaxCorners);
    fast_nonmax_with_scores(lev.im, lev.vCorners, 10, lev.vMaxCorners);

    // .. and then calculate the Shi-Tomasi scores of those, and keep the ones with
    // a suitably high score as Candidates, i.e. points which the mapmaker will attempt
    // to make new map points out of.
    std::vector<Candidate> vLevelCandidates;
    vLevelCandidates.reserve(lev.vMaxCorners.size());
    for(vector<pair<ImageRef,int> >::iterator i=lev.vMaxCorners.begin(); i!=lev.vMaxCorners.end(); i++)
    {
      if(!lev.im.in_image_with_border(i->first, 10))
        continue;
      double dSTScore = FindShiTomasiScoreAtPoint(lev.im, 3, i->first);
      if(dSTScore > *gvdCandidateMinSTScore)
      {
        // .. and then compute the scarf descriptor which is used for relocalisation
        Candidate c;
        c.irLevelPos = i->first;
        c.v2RootPos = LevelZeroPos(i->first, l);
        c.dSTScore = dSTScore;

        vLevelCandidates.push_back(c);
      }
    }

    int nNumLevelCandidates = min((int)vLevelCandidates.size(), *gvnMaxLevelCandidates);
    std::partial_sort(vLevelCandidates.begin(), vLevelCandidates.begin()+nNumLevelCandidates, vLevelCandidates.end());

    for(int i = 0; i < nNumLevelCandidates; ++i)
    {
      if(s.DescribePoint(int(vLevelCandidates[i].v2RootPos[0] + 0.5), int(vLevelCandidates[i].v2RootPos[1] + 0.5), 2*LevelScale(l), vLevelCandidates[i].descriptor))
        lev.vCandidates.push_back(vLevelCandidates[i]);
    }
    vLevelCandidates.clear();
  }

  // Also, make a SmallBlurryImage of the keyframe: The relocaliser uses these.
  pSBI = new SmallBlurryImage(*this);  
  // Relocaliser also wants the jacobians..
  pSBI->MakeJacs();
}

void KeyFrame::MakeKeyFrame_Colour(CVD::Image<CVD::Rgb<CVD::byte> > &colour)
{
  image.resize(colour.size());
  copy(colour, image);

  //Create texture for 3D rendering
  /*
  glEnable(GL_TEXTURE_2D);
  glGenTextures(1, &gluTextureId);
  glBindTexture(GL_TEXTURE_2D, gluTextureId);
  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image.size().x, image.size().y, 0, GL_RGB, GL_UNSIGNED_BYTE, image.data());
  glDisable(GL_TEXTURE_2D);
   */
}


void KeyFrame::ComputeGlobalFfromC(const SE3GaussianPDF &se3PdfGfromF)
{
  mse3PdfFfromC = mse3PdfGfromC - se3PdfGfromF; //MRPT style inverse composition
}


SE3GaussianPDF KeyFrame::ComputeGlobalWfromC(const SE3<> &se3FfromW)
{
  SE3GaussianPDF se3PdfWfromF(se3FfromW.inverse());
  return (se3PdfWfromF + mse3PdfFfromC); //MRPT style composition
}


// The keyframe struct is quite happy with default operator=, but Level needs its own
// to override CVD's reference-counting behaviour.
Level& Level::operator=(const Level &rhs)
{
  // Operator= should physically copy pixels, not use CVD's reference-counting image copy.
  im.resize(rhs.im.size());
  copy(rhs.im, im);
  
  vCorners = rhs.vCorners;
  vMaxCorners = rhs.vMaxCorners;
  vCornerRowLUT = rhs.vCornerRowLUT;
  return *this;
}

// -------------------------------------------------------------
// Some useful globals defined in LevelHelpers.h live here:
Vector<3> gavLevelColors[LEVELS];

// These globals are filled in here. A single static instance of this struct is run before main()
struct LevelHelpersFiller // Code which should be initialised on init goes here; this runs before main()
{
  LevelHelpersFiller()
  {
    for(int i=0; i<LEVELS; i++)
      {
	if(i==0)  gavLevelColors[i] = makeVector( 1.0, 0.0, 0.0);
	else if(i==1)  gavLevelColors[i] = makeVector( 1.0, 1.0, 0.0);
	else if(i==2)  gavLevelColors[i] = makeVector( 0.0, 1.0, 0.0);
	else if(i==3)  gavLevelColors[i] = makeVector( 0.0, 0.0, 0.7);
	else gavLevelColors[i] =  makeVector( 1.0, 1.0, 0.7); // In case I ever run with LEVELS > 4
      }
  }
};
static LevelHelpersFiller foo;
